﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading;

namespace CS633_Final_Project
{
    /// <summary>
    /// The MotionPlanner interface is used to define methods for pursuit of an evader.
    /// </summary>
    interface IMotionPlanner
    {
        /// <summary>
        /// Pursue the evader.
        /// </summary>
        void Move();

        /// <summary>
        /// Environment which the MotionPlanner plans for.
        /// </summary>
        Environment Playground
        {
            get;
            set;
        }

        /// <summary>
        /// Used to translate the robots smoothly.
        /// </summary>
        Visualization Visualizer
        {
            get;
            set;
        }
    }

    /// <summary>
    /// Motion Planner for the Evader, who moves arbitrarily.
    /// </summary>
    class EvaderPlanner : IMotionPlanner
    {
        private const int STEP_DISTANCE = 13;
        private Environment playground;
        private Visualization visualizer;

        /// <summary>
        /// Constructor for the EvaderPlanner
        /// </summary>
        /// <param name="playground">The environment of the EvaderPlanner.</param>
        public EvaderPlanner(Environment playground)
        {
            this.playground = playground;
            visualizer = new Visualization(playground);
        }

        /// <summary>
        /// Environment of the Evader Planner.
        /// </summary>
        public Environment Playground
        {
            get { return this.playground; }
            set { this.playground = value; }
        }

        /// <summary>
        /// Used to translate the robot smoothly.
        /// </summary>
        public Visualization Visualizer
        {
            get { return this.visualizer; }
            set { this.visualizer = value; }
        }

        /// <summary>
        /// Moves the evader around the environment until it is caught.
        /// </summary>
        public void Move()
        {
            while (!playground.Evader.Caught)
            {
                PointF randomLocation = findRandomLocation();
                visualizer.MoveEvader(playground.Evader, randomLocation);
                if (playground.SeePursuer())
                    playground.Evader.Caught = true;
            }
        }

        private PointF findRandomLocation()
        {
            Random random = new Random();
            PointF currLocation = playground.Evader.Location;
            PointF location = new PointF(0, 0);
            do
            {
                float x_operation = random.Next(-10, 10);
                float y_operation = random.Next(-10, 10);

                float x_change = random.Next(1, STEP_DISTANCE);
                float y_change = random.Next(1, STEP_DISTANCE);

                location.X = currLocation.X + x_operation * x_change;
                location.Y = currLocation.Y + y_operation * y_change;
            } while (!playground.InEnvironment(location) || !playground.validPath(currLocation, location));

            return location;
        }


    }

    /// <summary>
    /// Greedy method of visibility based pursuit-evasion algorithm
    /// </summary>
    class GreedyAreaPlanner : IMotionPlanner
    {
        private Visualization visualizer;
        private Environment playground;
        private const int NUM_RANDOM_MOVES = 5;

        /// <summary>
        /// Constructor for the greedy planner.
        /// </summary>
        /// <param name="playground">The environment of the greedy planner.</param>
        public GreedyAreaPlanner(Environment playground)
        {
            this.playground = playground;
            visualizer = new Visualization(playground);
        }

        /// <summary>
        /// The pursuit-based evasion environment.
        /// </summary>
        public Environment Playground
        {
            get { return this.playground; }
            set { this.playground = value; }
        }

        /// <summary>
        /// Used to translate the robot smoothly.
        /// </summary>
        public Visualization Visualizer
        {
            get { return this.visualizer; }
            set { this.visualizer = value; }
        }

        /// <summary>
        /// Pursue the evader.
        /// </summary>
        public void Move()
        {
            while (!playground.Evader.Caught)
            {
                for (int pursuerIndex = 0; pursuerIndex <= playground.Pursuers.Count - 1; pursuerIndex++)
                {
                    Pursue(pursuerIndex);
                    if (SeeEvader((Robot)playground.Pursuers.ToArray()[pursuerIndex]))
                    {
                        playground.Evader.Caught = true;
                        break;
                    }
                }
            }
        }

        private void Pursue(int pursuerIndex)
        {
            Robot pursuer = (Robot)playground.Pursuers.ToArray()[pursuerIndex];
            if (SeeEvader(pursuer))
                playground.Evader.Caught = true;
            else
            {
                if (AllPursuersStuck())
                {
                    Triangle locationTriangle = MoveRandomly(pursuer);
                    visualizer.MovePursuer(pursuer, locationTriangle);
                    pursuer.Stuck = false;
                }
                else
                {
                    Triangle oldLocation = pursuer.LocationTriangle;
                    Triangle newLocation = NextMove(pursuer);
                    visualizer.MovePursuer(pursuer, newLocation);
                    if (oldLocation.Equals(newLocation))
                        pursuer.Stuck = true;
                }
            }

        }

        private Triangle MoveRandomly(Robot pursuer)
        {
            return MoveRandomTimes(pursuer.LocationTriangle, NUM_RANDOM_MOVES);
        }

        private Triangle MoveRandomTimes(Triangle currentTriangle, int random_times)
        {
            if (random_times == 0)
                return currentTriangle;
            else
            {
                Random random = new Random();
                int random_edge_index = random.Next(0, 2);
                Line random_edge = currentTriangle.Edges[random_edge_index];
                ArrayList triangles = playground.TriangleDualGraph.GetTriangles(random_edge);
                foreach (Triangle tri in triangles)
                {
                    if (!currentTriangle.Equals(tri))
                        currentTriangle = tri;
                }
                Thread.Sleep(500);
                return MoveRandomTimes(currentTriangle, random_times - 1);
            }
        }

        private Triangle NextMove(Robot pursuer)
        {
            Robot movedPursuer = pursuer;
            Triangle currentTriangle = pursuer.LocationTriangle;

            foreach (Line edge in currentTriangle.Edges)
            {
                ArrayList triangles = playground.TriangleDualGraph.GetTriangles(edge);
                foreach (Triangle triangle in triangles)
                {
                    if (!currentTriangle.Equals(triangle)
                        && triangle.Area > currentTriangle.Area)
                        currentTriangle = triangle;
                }
            }
            return currentTriangle;
        }

        private bool SeeEvader(Robot pursuer)
        {
            return playground.SeeEvader(pursuer);
        }

        private bool AllPursuersStuck()
        {
            bool allStuck = true;

            foreach (Robot pursuer in playground.Pursuers)
            {
                if (!pursuer.Stuck)
                {
                    allStuck = false;
                    break;
                }
            }

            return allStuck;
        }
    }

    class TriangulatedVisibilityPlanner : IMotionPlanner
    {
        private Environment playground;
        private Visualization visualizer;

        /// <summary>
        /// Constructor for the TriangulatedVisibilityPlanner.
        /// </summary>
        /// <param name="playground">The pursuit-evasion environment.</param>
        public TriangulatedVisibilityPlanner(Environment playground)
        {
            this.playground = playground;
            this.visualizer = new Visualization(playground);
        }

        /// <summary>
        /// The pursuit-evasion environment.
        /// </summary>
        public Environment Playground
        {
            get { return this.playground; }
            set { this.playground = value; }
        }

        /// <summary>
        /// Used to smoothly move the pursuers around the environment.
        /// </summary>
        public Visualization Visualizer
        {
            get { return this.visualizer; }
            set { this.visualizer = value; }
        }

        /// <summary>
        /// Moves the pursuers to try and find the evader.
        /// </summary>
        public void Move()
        {
            while (!playground.Evader.Caught)
            {
                for (int pursuerIndex = 0; pursuerIndex <= playground.Pursuers.Count - 1; pursuerIndex++)
                {
                    pursue(pursuerIndex);
                    if (SeeEvader((Robot)playground.Pursuers.ToArray()[pursuerIndex]))
                    {
                        playground.Evader.Caught = true;
                        break;
                    }
                }
            }
        }

        private void pursue(int pursuerIndex)
        {
            Robot pursuer = (Robot)playground.Pursuers.ToArray()[pursuerIndex];

            // search the starting triangle
            searchTriangle(pursuer.LocationTriangle, pursuerIndex);

            // find a new triangle to move to.
            Triangle newLoc = pickNewTriangle(pursuerIndex);
            pursuer.LocationTriangle.Current_Status = Triangle.Status.RECONTAMINATED;
            visualizer.MovePursuer(pursuer, newLoc);


        }

        private void searchTriangle(Triangle startingLoc, int pursuerIndex)
        {

            Robot pursuer = (Robot)playground.Pursuers.ToArray()[pursuerIndex];
            foreach (Line edge in pursuer.LocationTriangle.Edges)
            {
                visualizer.MovePursuer(pursuer, edge.P);
                if (SeeEvader(pursuer))
                {
                    playground.Evader.Caught = true;
                    return;
                }
                Thread.Sleep(500);

                pursuer.Location = edge.Q;
                if (SeeEvader(pursuer))
                {
                    playground.Evader.Caught = true;
                    return;
                }
                Thread.Sleep(500);
            }

            pursuer.LocationTriangle = startingLoc;
            pursuer.LocationTriangle.Current_Status = Triangle.Status.CLEAR;
        }

        private Triangle pickNewTriangle(int pursuerIndex)
        {
            Robot pursuer = (Robot)playground.Pursuers.ToArray()[pursuerIndex];
            KeyValuePair<double, Triangle> winningValue;
            // 20% random chance to pick a random triangle
            Random random = new Random();
            int chance = random.Next(1, 5);

            if(chance == 1)
                return MoveRandom(pursuer.LocationTriangle);
            else
            {

            
            Triangle newTriangle = pursuer.LocationTriangle;
            ArrayList neighboringTriangles = getNeighboringTriangles(pursuer.LocationTriangle);

            // first see if there are any un-searched triangles.
            foreach (Triangle t in neighboringTriangles)
            {
                if (t.Current_Status.Equals(Triangle.Status.CONTAMINATED))
                {
                    return t;
                }
            }

            // find next best place to go
            ArrayList triangles = new ArrayList();
            foreach (Triangle t in neighboringTriangles)
            {
                KeyValuePair<double, Triangle> triangle = new KeyValuePair<double, Triangle>(getVisibilityArea(t), t);
                triangles.Add(triangle);
            }
            triangles.Sort(new VisibilityAreaComparer());
            winningValue = (KeyValuePair<double, Triangle>)triangles.ToArray()[0];
            }
            return winningValue.Value;
        }

        private ArrayList getNeighboringTriangles(Triangle triangle)
        {
            ArrayList triangles = new ArrayList();
            foreach (Line edge in triangle.Edges)
            {
                ArrayList neighboringTriangles = playground.TriangleDualGraph.GetTriangles(edge);
                foreach (Triangle t in neighboringTriangles)
                {
                    if (!t.Equals(triangle))
                        triangles.Add(t);
                }
            }
            return triangles;
        }

        public double getVisibilityArea(Triangle triangle)
        {
            double area = triangle.Area;
            foreach (Line edge in triangle.Edges)
            {
                ArrayList neighboringTriangles = playground.TriangleDualGraph.GetTriangles(edge);
                foreach (Triangle t in neighboringTriangles)
                {
                    if (!t.Equals(triangle))
                        area += t.Area;
                }
            }

            return area;
        }

        private bool SeeEvader(Robot pursuer)
        {
            return playground.SeeEvader(pursuer);
        }

        private Triangle MoveRandom(Triangle currentTriangle)
        {

            Random random = new Random();
            int random_edge_index = random.Next(0, 2);
            Line random_edge = currentTriangle.Edges[random_edge_index];
            ArrayList triangles = playground.TriangleDualGraph.GetTriangles(random_edge);
            foreach (Triangle tri in triangles)
            {
                if (!currentTriangle.Equals(tri))
                    currentTriangle = tri;
            }
            return currentTriangle;
        }
    }

    class VisibilityAreaComparer : IComparer
    {
        int IComparer.Compare(Object a, Object b)
        {
            KeyValuePair<double, Triangle> tri_a = (KeyValuePair<double, Triangle>)a;
            KeyValuePair<double, Triangle> tri_b = (KeyValuePair<double, Triangle>)b;

            return tri_b.Key.CompareTo(tri_a.Key);
        }
    }
}
